#include "srv_control.h"
#include "drv_motor.h"
#include "drv_encoder.h"
#include "rtthread.h"

#include "math.h"

#include <stdio.h>

#define SPEED_TIME_INTERVAL 10

#define COUNT_PRE_METER 6153

static double final_speed = 0;
static motor_speed_t except_speed = {0};
static motor_speed_t current_output_speed = {0};
static double turn_speed_add = 0;

// 左右电机速度不对称....

// #define LEFT_MOTOR_SPEED_ERR -0.4;

static void _apply_motor(void)
{
    // double ratio = (double) turn_speed_add/final_speed;
    except_speed.left = (final_speed - turn_speed_add ) ;
    except_speed.right = (final_speed + turn_speed_add ) ;

    ist_motor_set(MOTOR_LEFT, except_speed.left);
    ist_motor_set(MOTOR_RIGHT,except_speed.right);
}

void ist_ctr_set_normal_speed(double speed)
{
    final_speed = speed;
    _apply_motor();
}

void ist_ctr_set_turn(double turn)
{
    if (turn > 100)
    {
        turn = 100;
    }
    else if (turn < -100)
    {
        turn = -100;
    }
    turn_speed_add = turn;
    _apply_motor();
}

void ist_ctr_speed_loop(void *param)
{
}

void ist_ctr_init(void)
{
    ist_motor_init();
    // ist_encoder_init();

    ist_ctr_set_normal_speed(0);
    ist_ctr_set_turn(0);

    // 启动一个定时器，实现速度闭环
    // rt_timer_t timer = rt_timer_create("speed_loop", ist_ctr_speed_loop, RT_NULL, SPEED_TIME_INTERVAL, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
    // rt_timer_start(timer);
}

INIT_COMPONENT_EXPORT(ist_ctr_init);
